import cv2
import numpy as np
import math
import serial

def Routing(point1,point2,remap_point,image):
    p1 = remap_point[point1[0]][point1[1]]
    p2 = remap_point[point2[0]][point2[1]]
    #cv2.circle(im_crop, p1, 2, (0, 0, 255), 2)  # 画圆心
    #cv2.circle(im_crop, p2, 2, (0, 0, 255), 2)  # 画圆心
    #如果x轴相等，那就是y轴需要移动
    if(p1[0]==p2[0]):
            if p2[1]>p1[1]:
                a = image[p1[1] :p2[1],p1[0] ]> 0
            else:
                a = image[p2[1]:p1[1], p1[0]] > 0
            sum = np.sum(a)
            if sum>0 :
                return False
    if (p1[1] == p2[1]):
        if(p1[0]>p2[0]):
            a = image[p1[1], p2[0]:p1[0]] > 0
        else:
            a = image[p1[1], p1[0]:p2[0]] > 0
        sum = np.sum(a)
        if sum > 0:
            return False
    return True
def get_route(point0,point1,im_crop,edge,points):
    bianli_list = []
    route = []
    head = 0#队列头
   # rear = 0#队列尾巴
    bianli_list.append([point0[0],point0[1],-1,1])
    while bianli_list[head][0]!=point1[0] or bianli_list[head][1]!=point1[1]:
        x = bianli_list[head][0]
        y = bianli_list[head][1]
        r = bianli_list[head][3]
        point_lf = [x-1,y]
        point_rg = [x + 1, y]
        point_up = [x, y-1]
        point_dn = [x, y+1]
        if 0<=point_lf[0] < 10 and 0<=point_lf[1] < 10:
                if point_lf[0] != bianli_list[bianli_list[head][2]][0] or point_lf[1] != bianli_list[bianli_list[head][2]][1]:
                    if Routing([x,y],point_lf,points,edge)==True:
                        bianli_list.append([point_lf[0],point_lf[1],head,r+1])

        if 0 <= point_rg[0] < 10 and 0 <= point_rg[1] < 10:
            if point_rg[0] != bianli_list[bianli_list[head][2]][0] or point_rg[1] != bianli_list[bianli_list[head][2]][ 1]:
                if Routing([x,y],point_rg,points,edge)==True:
                    bianli_list.append([point_rg[0],point_rg[1],head,r+1])

        if 0 <= point_up[0] < 10 and 0 <= point_up[1] < 10:
            if point_up[0] != bianli_list[bianli_list[head][2]][0] or point_up[1] != bianli_list[bianli_list[head][2]][1]:
                if Routing([x,y],point_up,points,edge)==True:
                    bianli_list.append([point_up[0],point_up[1],head,r+1])


        if 0 <= point_dn[0] < 10 and 0 <= point_dn[1] < 10:
            if point_dn[0] != bianli_list[bianli_list[head][2]][0] or point_dn[1] != bianli_list[bianli_list[head][2]][1]:
                if Routing([x,y],point_dn,points,edge)==True:
                    bianli_list.append([point_dn[0],point_dn[1],head,r+1])
        head+=1

    while(bianli_list[head][2]!=-1):
        x0 = bianli_list[head][0]
        y0 = bianli_list[head][1]
        route.append([x0,y0])
        pre_index = bianli_list[head][2]
        x1 = bianli_list[pre_index][0]
        y1 = bianli_list[pre_index][1]
        cv2.line(im_crop, (points[x0][y0][0],points[x0][y0][1]), (points[x1][y1][0],points[x1][y1][1]), (0, 0, 255), 2)
        head = pre_index
    return route
        # cv2.imshow("line",im_crop)
        # cv2.waitKey()
def get_short_route(point0,baozang_points,im_crop,edge,points):
    bianli_list = []
    head = 0#队列头
    route = []
    total_route = []
   # rear = 0#队列尾巴
    bianli_list.append([point0[0],point0[1],-1,1])
    while(baozang_points):
        while [bianli_list[head][0],bianli_list[head][1]] not in baozang_points:
            x = bianli_list[head][0]
            y = bianli_list[head][1]
            r = bianli_list[head][3]
            point_lf = [x-1,y]
            point_rg = [x + 1, y]
            point_up = [x, y-1]
            point_dn = [x, y+1]
            if 0<=point_lf[0] < 10 and 0<=point_lf[1] < 10:
                    if point_lf[0] != bianli_list[bianli_list[head][2]][0] or point_lf[1] != bianli_list[bianli_list[head][2]][1]:
                        if Routing([x,y],point_lf,points,edge)==True:
                            bianli_list.append([point_lf[0],point_lf[1],head,r+1])

            if 0 <= point_rg[0] < 10 and 0 <= point_rg[1] < 10:
                if point_rg[0] != bianli_list[bianli_list[head][2]][0] or point_rg[1] != bianli_list[bianli_list[head][2]][ 1]:
                    if Routing([x,y],point_rg,points,edge)==True:
                        bianli_list.append([point_rg[0],point_rg[1],head,r+1])

            if 0 <= point_up[0] < 10 and 0 <= point_up[1] < 10:
                if point_up[0] != bianli_list[bianli_list[head][2]][0] or point_up[1] != bianli_list[bianli_list[head][2]][1]:
                    if Routing([x,y],point_up,points,edge)==True:
                        bianli_list.append([point_up[0],point_up[1],head,r+1])


            if 0 <= point_dn[0] < 10 and 0 <= point_dn[1] < 10:
                if point_dn[0] != bianli_list[bianli_list[head][2]][0] or point_dn[1] != bianli_list[bianli_list[head][2]][1]:
                    if Routing([x,y],point_dn,points,edge)==True:
                        bianli_list.append([point_dn[0],point_dn[1],head,r+1])
            head+=1
        new_start_point = [bianli_list[head][0],bianli_list[head][1]]
        baozang_points.remove(new_start_point)
        while(bianli_list[head][2]!=-1):
            x0 = bianli_list[head][0]
            y0 = bianli_list[head][1]
            route.append([x0,y0])
            pre_index = bianli_list[head][2]
            x1 = bianli_list[pre_index][0]
            y1 = bianli_list[pre_index][1]
            cv2.line(im_crop, (points[x0][y0][0],points[x0][y0][1]), (points[x1][y1][0],points[x1][y1][1]), (51, 255, 255), 2) # BGR黄色
            head = pre_index
        route.append([bianli_list[head][0],bianli_list[head][1]])
        total_route.append(route[:])
        route.clear()
        bianli_list.clear()
        bianli_list.append([new_start_point[0],new_start_point[1],-1,1])
    route = get_route([9,0],new_start_point,im_crop,edge,points)
    total_route.append(route)
    return im_crop,total_route


def find_rect(img):
        edge =cv2.Canny(img,200,250);
        #binaly_img   = cv2.dilate(edge, kernel, iterations = 1)
        contours = cv2.findContours(edge,cv2.RETR_TREE,cv2.CHAIN_APPROX_NONE)
        
        #3print("-----------------------------")
        #print((contours[2]))
        bounding_boxes = [cv2.boundingRect(cnt) for cnt in contours[1]]
        index_list = []
        #print(contours[2][0])
        #找到有内轮廓的轮廓
        for i in range (len(bounding_boxes)):
             hie = contours[2][0][i]
             j=0;
             while(hie[2]>0):
                 j+=1
                 hie = contours[2][0][hie[2]]
             if j>2:
               index_list.append(i)
             
        for i,box in enumerate(bounding_boxes):
            if i in index_list:
                if box[2]>1.5*box[3] or box[2]<0.6*box[3] or box[3]*box[2]>10000:
                    index_list.remove(i)
                
                #else:
                  #  cv2.rectangle(img, (box[0], box[1]), (box[0] + box[2], box[1] + box[3]), (0, 255, 0), 2)
        
        nei_list = []    
        for i in index_list:
             hie = contours[2][0][i]
             j=0;
             while(hie[2]>0):   
                 hie = contours[2][0][hie[2]]
                 h1 =  bounding_boxes[i][3]
             nei_list.append(hie[3])
             
        nei_list.sort()
        
        index_list = []
        for l in nei_list:
            if l not in index_list:
                index_list.append(l)
                box = bounding_boxes[l]
                cv2.rectangle(img, (box[0], box[1]), (box[0] + box[2], box[1] + box[3]), (0, 255, 0), 2)
        
        points =[]
        cv2.imshow("img",img)
        #cv2.waitKey(10)
        if len(index_list)>=4:
             for l in index_list:
                x,y,w,h = bounding_boxes[l]
                if w*h<400:
                    points.append([int(x+w/2),int(y+h/2)])
                   
        if(len(points)==4):
               points.sort()
               if(points[0][1]<points[1][1]):
                      left_top_p = points[0]
                      left_down_p = points[1]
               else:
                      left_top_p = points[1]
                      left_down_p = points[0]
                      
               
               if(points[2][1]<points[3][1]):
                      right_top_p = points[2]
                      right_down_p = points[3]
               else:
                      right_top_p = points[3]
                      right_down_p = points[2]
            
               return left_top_p,left_down_p,right_top_p,right_down_p,h1
        return 0,0,0,0,0   

def img_jiaozheng(left_top_p,left_down_p,right_top_p,right_down_p,frame,h1):
       #if (right_down_p[0]-left_top_p[0])>(right_down_p[1]-left_top_p[1]):
         #  w = right_down_p[0]-left_top_p[0]
       #else:
          # w = right_down_p[1]-left_top_p[1]
    tl = left_top_p
    tr = right_top_p
    br = right_down_p
    bl = left_down_p
    
    widthA = np.sqrt(((br[0] - bl[0])**2)+((br[1] - bl[1])**2))
    widthB = np.sqrt(((tr[0] - tl[0])**2)+((tr[1] - tl[1])**2))
    maxWidth = max(int(widthA),int(widthB))
    
    heightA = np.sqrt(((tr[0] - br[0])**2)+((tr[1] - br[1])**2))
    heightB = np.sqrt(((tl[0] - bl[0])**2)+((tl[1] - bl[1])**2))
    maxheight = max(int(heightA),int(heightB))
    
    target_lf_t = [0,0]
    target_rg_t =[maxWidth-1,0]
    target_rg_d = [maxWidth-1,maxheight-1]
    target_lf_d = [0,maxheight-1]

    #开始校正
    pointSrc = np.float32([tl, tr, br, bl])  # 原始图像中 4点坐标
    pointDst = np.float32([target_lf_t, target_rg_t, target_rg_d, target_lf_d])  # 变换图像中 4点坐标
    MP = cv2.getPerspectiveTransform(pointSrc, pointDst)  # 计算投影变换矩阵 M
    imgP = cv2.warpPerspective(frame, MP, (maxWidth,maxheight))
    im_crop = imgP[int(maxheight/22):maxheight-int(maxheight/22),int(maxWidth/22):maxWidth-int(maxWidth/22),:]     
     
    cv2.imshow("img",im_crop) #矫正后的图片      
    cv2.waitKey(10)
          
    return im_crop


def get_points(im_crop):
   gray_image = cv2.cvtColor(im_crop,cv2.COLOR_RGB2GRAY)
   ret,edge = cv2.threshold(gray_image,70,255,cv2.THRESH_BINARY_INV)
   #edge =cv2.Canny(gray_image,200,250)+edge;
   kernel = np.ones((3, 3), np.uint8)
   baozang_points = []
   points=[]
   edge   = cv2.dilate(edge, kernel, iterations = 1)
   
   kernel = np.ones((1, int(im_crop.shape[1]/30)), np.uint8)
   edge1 = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kernel,  iterations=2);
   lines = cv2.HoughLines(edge1, 1, np.pi / 2, 80,200)
   mid_p_x = []
   if 1:
       lines1 = lines[:, 0, :]
      # mid_p_x = []
       for rho, theta in lines1[:]:
            a = np.cos(theta)
            b = np.sin(theta)  
            y0 = b * rho
            if y0>0:
               mid_p_x.append(y0)

   kernel = np.ones((int(im_crop.shape[0]/30), 1), np.uint8)
   binaly_img2 = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kernel,  iterations=2);
   lines =  cv2.HoughLines(binaly_img2, 1, np.pi / 2, 50,150)
   #lines1 = lines[:, 0, :]
   mid_p_y = []
   if 1:
      
       lines1 = lines[:, 0, :]
       for rho, theta in lines1[:]:
            a = np.cos(theta)
            b = np.sin(theta)
            x0 = a * rho
            if x0>0:
                mid_p_y.append(x0)
   
   if(len(mid_p_y)>5 and len(mid_p_x)>5):
        mid_p_y.sort()
        mid_p_x.sort()
        len_x = len(mid_p_x)
        len_y = len(mid_p_y)
        gap_x=[]
        gap_y=[]
        for i in range(len_x-1):
            gap_x.append(mid_p_x[i+1]-mid_p_x[i])
        for i in range(len_y-1):
            gap_y.append(mid_p_y[i+1]-mid_p_y[i])
        width,height,c =    im_crop.shape
        gap_av_x = width/12#np.mean(gap_x)
        gap_av_y = height/12#np.mean(gap_y)
        remove_x=[]
        remove_y=[]
        for i,g in enumerate(gap_x):
            if g<gap_av_x*0.7:
              remove_x.append(mid_p_x[i+1])
            if g>gap_av_x*1.5:
              mid_p_x.append(mid_p_x[i]+int((mid_p_x[i+1]-mid_p_x[i])/2))
        for i ,g in enumerate(gap_y):
            if g<gap_av_y*0.7:
              remove_y.append(mid_p_y[i+1])
            if g>gap_av_y*1.5:
              mid_p_y.append(mid_p_y[i]+int((mid_p_y[i+1]-mid_p_y[i])/2))
        for x in remove_x:
            mid_p_x.remove(x)
        for y in remove_y:
            mid_p_y.remove(y)
        im = im_crop.copy()
        for x in mid_p_y:
             cv2.line(im, (x, 0), (x, 600), (0, 0, 255), 2)
        for x in mid_p_x:
             cv2.line(im, (0, x), (600, x), (0, 0, 255), 2)
        
        if len(mid_p_x)==11 and len(mid_p_y)==11:
            points=[]
            mid_p_y.sort();
            mid_p_x.sort();
            for i in range(10):
                x_list = []
                y = mid_p_y[i]
                for j in range(10):
                    x = mid_p_x[j]
                    x_list.append([int(y+gap_av_y/2),int(x+gap_av_x/2)])
                    #cv2.circle(im_crop, (int(y+gap_av_y/2),int(x+gap_av_x/2)), 3, [0, 0, 255])

                points.append(x_list)
            kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(int(gap_av_y/3),int(gap_av_x/3)))
            yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel,  iterations=1);
            #circles = cv2.HoughCircles(edge, cv2.HOUGH_GRADIENT, 1, 30, param1=100, param2=200, minRadius=0, maxRadius=0)
            contours = cv2.findContours(yuan,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
            bounding_boxes = [cv2.boundingRect(cnt) for cnt in contours[1]]
            
            for bbox in bounding_boxes:
                  [x,y,w,h] = bbox
                
                #baozang_point.append([int(x+w/2),int(y+h/2)])
                  if (x>mid_p_y[0]) and ((x+w/2)<mid_p_y[10]) :
                    for i in range(10):
                        if (x-points[i][0][0])<gap_av_y/2:
                            break
                          
                    for j in range(10):
                        if (y-points[0][j][1])<gap_av_x/2:
                            baozang_points.append([i,j]);
                            break
                
                    for p in baozang_points:
                        x = points[p[0]][p[1]]              
                        cv2.circle(im, (x[0],x[1]), 5, [255, 0, 0]) 
        else:
            kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(int(gap_av_y/3),int(gap_av_x/3)))
            yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel,  iterations=1);
            
      
   else:
            kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(int(30/3),int(30/3)))
            yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel,  iterations=1);
            
                    #return binaly_img2+edge1,points,baozang_point
                    #cv2.imshow("yuan",yuan)
                    #cv2.waitKey(10)
   kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(3,3))
   edge1 = binaly_img2+edge1+cv2.morphologyEx(edge-yuan, cv2.MORPH_OPEN, kenel,  iterations=1);
            #edge1 = yuan
   #cv2.imshow('shange',im) # 迷宫格
   cv2.waitKey(10)
   # cv2.imshow("edge1",edge1) #边缘图像
   cv2.waitKey(10)        
        #cv2.circle(im_crop, (int(y+gap_av_y/2),int(x+gap_av_x/2)), 5, [0, 0, 255])
   #edge2=binaly_img2+edge1
   return points,baozang_points,edge1
            
            
            #cv2.line(im_crop, (mid_p_y[0], 0), (mid_p_y[0], 600), (0, 0, 255), 2)
            
            #cv2.line(im_crop, (mid_p_y[10], 0), (mid_p_y[10], 600), (0, 0, 255), 2)
   

        
def del_img():
    kernel = np.ones((3, 3), np.uint8)
    cap=cv2.VideoCapture(0)  #调用摄像头‘0’一般q是打开电脑自带摄像头，‘1’是打开外部摄像头（只有一个摄像头的情况）
    width=800
    height=600
    cap.set(cv2.CAP_PROP_FRAME_WIDTH,width)#设置图像宽度
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT,height)#设置图像高度
    #显示图像
    while True: 
        ret,frame=cap.read()#读取图像(frame就是读取的视频帧，对frame处理就是对整个视频的处理)
        #print(ret)#
        #######例如将图像灰度化处理，
        img=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)#转灰度图
        # cv2.imshow("imcrop",img)
        input=cv2.waitKey(10)
        #gray_image = cv2.medianBlur(img,3)
        left_top_p,left_down_p,right_top_p,right_down_p,h1=find_rect(img)
        if(left_top_p!=0):    
            im_crop = img_jiaozheng(left_top_p,left_down_p,right_top_p,right_down_p,frame,h1)              
            cv2.imshow("jiaozheng",im_crop) #矫正后的图片
            input=cv2.waitKey(10)
            points,baozang_point,edge=get_points(im_crop)
            if len(baozang_point)==8:
                
        
                return edge,points,baozang_point,im_crop
        #return 0,0,0
def route_convert_instruct(route):
    instruct_t=[]
    instruct = []
    instruct.append(3)
    pre_dir = [1,0]
    for r in route:
        instruct = []
        length = len(r)
        for i in range(2,len(r)):
            x = [r[length-i-1][0]-r[length-i][0],r[length-i-1][1]-r[length-i][1]]
            if(x==pre_dir):
               instruct.append(3)
            elif(x[0]<0 and pre_dir[1]>0):
               instruct.append(0)
            elif(x[0]<0 and pre_dir[1]<0):
               instruct.append(1)
            elif(x[0]>0 and pre_dir[1]>0):
               instruct.append(1)
            elif(x[0]>0 and pre_dir[1]<0):
               instruct.append(0)
            elif(x[1]<0 and pre_dir[0]<0):
               instruct.append(0)
            elif(x[1]<0 and pre_dir[0]>0):
               instruct.append(1)  
            elif(x[1]>0 and pre_dir[0]>0):
               instruct.append(0)
            elif(x[1]>0 and pre_dir[0]<0):
               instruct.append(1)
            pre_dir = x
       # instruct.append(3)
        instruct.append(2)
        pre_dir =  [-pre_dir[0],-pre_dir[1]]
        instruct_t.append(instruct[:])
        instruct.clear()
    return instruct_t
def send_data(ser,ins):
    for d in ins:
        for i in d:
            if(i==0):
                ser.write(b'0')#turn right
            if(i==1):
                ser.write(b'1')#turn left
            if(i==2):
                ser.write(b'2')#turn back
            if(i==3):
                ser.write(b'3')#go farward
        
        ser.write(b'4') #shibiebaozang
            
    ser.write(b'5')#tingche
            
def main():
    #ser = serial.Serial("/dev/ttyAMA0",115200) #启动串口
    #ser.write(b'')
    edge,points,baozang_point,im_crop = del_img()
    for point in baozang_point:
        if(len(baozang_point)==8):
            im_crop1,total_r = get_short_route([0,9],baozang_point,im_crop,edge,points)
            ins = route_convert_instruct(total_r)
            # send_data(ser,ins)  #通过串口发送数据
            print(ins)


    while(True):
        cv2.imshow('lujing',im_crop1) # 路径显示
        input=cv2.waitKey(10)
        if input==ord('q'):
             break
    #cap.release()#释放摄像头
#cv2.destroyAllWindows()#销毁窗口
            
if __name__=='__main__':
    main()